/*
 * Implementation file for joints between rigid bodies.
 */
#include "PhxJoints.h"

using namespace phx;

unsigned Joint::AddContact(Contact* contact, unsigned limit) const
{
	// Calculate the position of each connection point in world coordinates.
	Vector3 positionOneWorld = mBodies[0]->GetPointInWorldSpace(mPositions[0]);
	Vector3 positionTwoWorld = mBodies[1]->GetPointInWorldSpace(mPositions[1]);

	// Calculate the length of the joint.
	Vector3 oneToTwo = positionTwoWorld - positionOneWorld;
	Vector3 normal = oneToTwo;
	normal.Normalise();
	real length = oneToTwo.Magnitude();

	// Check if it is violated.
	if (real_abs(length) > mError)
	{
		contact->mBodies[0] = mBodies[0];
		contact->mBodies[1] = mBodies[1];
		contact->mContactNormal = normal;
		contact->mContactPoint = (positionOneWorld + positionTwoWorld) * 0.5f;
		contact->mPenetration = length - mError;
		contact->mFriction = 1.0f;
		contact->mRestitution = 0;
		return 1;
	}

	return 0;
}

void Joint::Set(RigidBody* bodyOne, const Vector3& positionOne, RigidBody* bodyTwo , const Vector3& positionTwo, real error)
{
	mBodies[0] = bodyOne;
	mBodies[1] = bodyTwo;

	mPositions[0] = positionOne;
	mPositions[1] = positionTwo;

	mError = error;
}